#include <Eigen/Eigen>
#include <bits/stdc++.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/State.h>
#include <opencv2/opencv.hpp>
#include <ros/ros.h>

cv::Mat Get3DR_TransMatrix(const std::vector<cv::Point3f> &srcPoints,
                           const std::vector<cv::Point3f> &dstPoints) {
  double srcSumX = 0.0f;
  double srcSumY = 0.0f;
  double srcSumZ = 0.0f;

  double dstSumX = 0.0f;
  double dstSumY = 0.0f;
  double dstSumZ = 0.0f;

  //至少三组点
  if (srcPoints.size() != dstPoints.size() || srcPoints.size() < 3) {
    return cv::Mat();
  }

  int pointsNum = srcPoints.size();
  for (int i = 0; i < pointsNum; ++i) {
    srcSumX += srcPoints[i].x;
    srcSumY += srcPoints[i].y;
    srcSumZ += srcPoints[i].z;

    dstSumX += dstPoints[i].x;
    dstSumY += dstPoints[i].y;
    dstSumZ += dstPoints[i].z;
  }

  cv::Point3d centerSrc, centerDst;

  centerSrc.x = double(srcSumX / pointsNum);
  centerSrc.y = double(srcSumY / pointsNum);
  centerSrc.z = double(srcSumZ / pointsNum);

  centerDst.x = double(dstSumX / pointsNum);
  centerDst.y = double(dstSumY / pointsNum);
  centerDst.z = double(dstSumZ / pointsNum);

  // Mat::Mat(int rows, int cols, int type)
  cv::Mat srcMat(3, pointsNum, CV_64FC1);
  cv::Mat dstMat(3, pointsNum, CV_64FC1);
  // ---Modify
  for (int i = 0; i < pointsNum; ++i) // N组点
  {
    //三行
    srcMat.at<double>(0, i) = srcPoints[i].x - centerSrc.x;
    srcMat.at<double>(1, i) = srcPoints[i].y - centerSrc.y;
    srcMat.at<double>(2, i) = srcPoints[i].z - centerSrc.z;

    dstMat.at<double>(0, i) = dstPoints[i].x - centerDst.x;
    dstMat.at<double>(1, i) = dstPoints[i].y - centerDst.y;
    dstMat.at<double>(2, i) = dstPoints[i].z - centerDst.z;
  }

  cv::Mat matS = srcMat * dstMat.t();

  cv::Mat matU, matW, matV;
  cv::SVDecomp(matS, matW, matU, matV);

  std::cout<<"opencv matS:\n"<<matS<<std::endl;
  std::cout<<"opencv matU:\n"<<matU<<std::endl;
  std::cout<<"opencv matV:\n"<<matV<<std::endl;
  std::cout<<"opencv matW:\n"<<matW<<std::endl;

  cv::Mat matTemp = matU * matV;
  double det = cv::determinant(matTemp); //行列式的值

  std::cout<<"opencv det:"<<det<<std::endl;

  double datM[] = {1, 0, 0, 0, 1, 0, 0, 0, det};
  cv::Mat matM(3, 3, CV_64FC1, datM);

  cv::Mat matR = matV.t() * matM * matU.t();

  std::cout<<"opencv matR:\n"<<matR<<std::endl;


  double *datR = (double *)(matR.data);
  double delta_X =
      centerDst.x -
      (centerSrc.x * datR[0] + centerSrc.y * datR[1] + centerSrc.z * datR[2]);
  double delta_Y =
      centerDst.y -
      (centerSrc.x * datR[3] + centerSrc.y * datR[4] + centerSrc.z * datR[5]);
  double delta_Z =
      centerDst.z -
      (centerSrc.x * datR[6] + centerSrc.y * datR[7] + centerSrc.z * datR[8]);

  //生成RT齐次矩阵(4*4)
  cv::Mat R_T =
      (cv::Mat_<double>(4, 4) << matR.at<double>(0, 0), matR.at<double>(0, 1),
       matR.at<double>(0, 2), delta_X, matR.at<double>(1, 0),
       matR.at<double>(1, 1), matR.at<double>(1, 2), delta_Y,
       matR.at<double>(2, 0), matR.at<double>(2, 1), matR.at<double>(2, 2),
       delta_Z, 0, 0, 0, 1);

  return R_T;
}

Eigen::Matrix4d CalculateMatrix(const std::vector<Eigen::Vector3d> &srcPoints,
                     const std::vector<Eigen::Vector3d> &dstPoints) {
  double srcSumX = 0.0f;
  double srcSumY = 0.0f;
  double srcSumZ = 0.0f;

  double dstSumX = 0.0f;
  double dstSumY = 0.0f;
  double dstSumZ = 0.0f;

  int pointsNum = srcPoints.size();
  for (int i = 0; i < pointsNum; ++i) {
    srcSumX += srcPoints[i].x();
    srcSumY += srcPoints[i].y();
    srcSumZ += srcPoints[i].z();

    dstSumX += dstPoints[i].x();
    dstSumY += dstPoints[i].y();
    dstSumZ += dstPoints[i].z();
  }

  Eigen::Vector3d centerSrc, centerDst;
  centerSrc.x() = double(srcSumX / pointsNum);
  centerSrc.y() = double(srcSumY / pointsNum);
  centerSrc.z() = double(srcSumZ / pointsNum);

  centerDst.x() = double(dstSumX / pointsNum);
  centerDst.y() = double(dstSumY / pointsNum);
  centerDst.z() = double(dstSumZ / pointsNum);


  // Mat::Mat(int rows, int cols, int type)
  Eigen::Matrix3d srcMat;
  Eigen::Matrix3d dstMat;
  for (int i = 0; i < pointsNum; ++i) {
    srcMat.col(i) = srcPoints[i] - centerSrc;

    dstMat.col(i) = dstPoints[i] - centerDst;
  }

  Eigen::Matrix3d matS = srcMat * dstMat.transpose();

  Eigen::Matrix3d matU, matW, matV;
  // cv::SVDecomp(matS, matW, matU, matV);
  Eigen::JacobiSVD<Eigen::MatrixXd> svd(matS, Eigen::ComputeFullU | Eigen::ComputeFullV);

  matU = svd.matrixU();
  matV = svd.matrixV();
  matW = matU.inverse() * matS * matV.transpose().inverse();

  std::cout<<"eigen matS:\n"<<matS<<std::endl;
  std::cout<<"eigen matU:\n"<<matU<<std::endl;
  std::cout<<"eigen matV:\n"<<matV<<std::endl;
  std::cout<<"eigen matW:\n"<<matW<<std::endl;

  Eigen::Matrix3d matTemp = matU * matV.transpose();
  double det = matTemp.determinant(); //行列式的值
  std::cout<<"eigen det:"<<det<<std::endl;

  Eigen::Matrix3d matM;
  matM<<1, 0, 0, 
        0, 1, 0, 
        0, 0, det;

  Eigen::Matrix3d matR = (matU* matM * matV.transpose()).transpose();
  std::cout<<"eigen matR:\n"<<matR<<std::endl;
  std::cout<<"matR,DETER:\n"<<matR.determinant()<<std::endl;

  double delta_X =
      centerDst.x() -
      (centerSrc.x() * matR(0,0) + centerSrc.y() * matR(0,1) + centerSrc.z() * matR(0,2));
  double delta_Y =
      centerDst.y() -
      (centerSrc.x() * matR(1,0) + centerSrc.y() * matR(1,1) + centerSrc.z() * matR(1,2));
  double delta_Z =
      centerDst.z() -
      (centerSrc.x() * matR(2,0) + centerSrc.y() * matR(2,1) + centerSrc.z() * matR(2,2));

  //生成RT齐次矩阵(4*4)
  Eigen::Matrix4d R_T;
  R_T<< matR(0, 0),matR(0, 1),matR(0, 2),delta_X,
        matR(1, 0),matR(1, 1),matR(1, 2),delta_Y,
        matR(2, 0),matR(2, 1),matR(2, 2),delta_Z,
        0,0,0,1;


  Eigen::Quaterniond current_q=Eigen::Quaterniond(matR);
  std::cout<<"current_q:"<<current_q.x()<<" "<<current_q.y()<<" "<<current_q.z()<<" "<<current_q.w()<<std::endl;

  Eigen::Vector3d current_p(delta_X,delta_Y,delta_Z);

  return R_T;
}

int main(int argc, char **argv) {
  ros::init(argc, argv, "calculate_quaternion");
  ros::NodeHandle nh("~");



  {
    std::vector<cv::Point3f> srcPoints;
    std::vector<cv::Point3f>  dstPoints;
  
  //模拟绕Z轴转90度，再X轴平移50.
  //取三组点
    float NN = 100;
    srcPoints.push_back(cv::Point3f(NN, 0, 0));
    dstPoints.push_back(cv::Point3f(50+0, NN, 0));
  
    srcPoints.push_back(cv::Point3f(0, NN, 0));
    dstPoints.push_back(cv::Point3f(50 -NN, 0, 0));
  
    srcPoints.push_back(cv::Point3f(0, 0, NN));
    dstPoints.push_back(cv::Point3f(50+0, 0, NN));
  
    cv::Mat RT = Get3DR_TransMatrix(srcPoints, dstPoints);
    for (int r = 0; r < RT.rows; r++)
    {
      for (int c = 0; c < RT.cols; c++)
      {
        printf("%f, ", RT.at<double>(r, c));
      }
      printf("\n");
    }
    
  }


  std::vector<Eigen::Vector3d> srcPoints;
	std::vector<Eigen::Vector3d>  dstPoints;
 
//模拟绕Z轴转90度，再X轴平移50.
//取三组点
	float NN = 100;
	srcPoints.push_back(Eigen::Vector3d(NN, 0, 0));
	dstPoints.push_back(Eigen::Vector3d(50+0, NN, 0));
 
	srcPoints.push_back(Eigen::Vector3d(0, NN, 0));
	dstPoints.push_back(Eigen::Vector3d(50 -NN, 0, 0));
 
	srcPoints.push_back(Eigen::Vector3d(0, 0, NN));
	dstPoints.push_back(Eigen::Vector3d(50+0, 0, NN));
 
	Eigen::Matrix4d RT = CalculateMatrix(srcPoints, dstPoints);

  std::cout<<"RT: "<<RT<<std::endl;


  return 0;
}